#ifndef __SERIAL32_H__
#define __SERIAL32_H__

#include "rclcpp/rclcpp.hpp"
#include "serial/serial.h"
#include <iostream>
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/range.hpp"

typedef struct
{
    uint8_t head;
    uint8_t addr;
    uint8_t ID;
    uint8_t dataLen;
    uint8_t dataBuf[256];
    uint8_t sc1;
    uint8_t sc2;
}__attribute__ ((__packed__)) myRxStruct;	 



typedef union 
{
    myRxStruct frame;
    uint8_t rawBytes[sizeof(myRxStruct)];
}__attribute__ ((__packed__)) myRxFrame;




typedef struct
{
	uint8_t _isUsed;
	uint8_t _linkType;			
}__attribute__ ((__packed__)) myTxStruct;


//用于发送的结构体，data保存帧内容，cmd保存发送参数
typedef struct
{
	myRxFrame data;		
	myTxStruct cmd; 			
}__attribute__ ((__packed__)) myTxFrame;





class Serial32 : public rclcpp::Node
{
public:
    serial::Serial ser;
    serial::Timeout _timeout;
    myRxFrame RxFrame;
    myRxFrame TxFrame;
    uint8_t read_buf[2048];


    
    Serial32(std::string Port = "/dev/ttyS9",  uint32_t Baudrate = 115200,  uint32_t timeout = 1000, const std::string & node_name = "Serial32");
    ~Serial32();
    


    bool Ser_ReceiveOneByte(uint8_t Byte);
    bool Rx_Frame_Anl(void);
    bool Read_SerialData(void);
    void SendSpeedTo32(int16_t Lspeed, int16_t Rspeed);
    void Ser_Send(uint8_t linktype, uint8_t *buf, uint16_t len);
    void AddTxFrame(uint8_t linkType, myRxFrame *pFrame);
    void SendCheck(void);
    void SendBuf(const uint8_t ID,const uint8_t* buf, const uint8_t len);

    void timer_callback();
    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
    void pub_alt(void);


    rclcpp::TimerBase::SharedPtr timer_;  
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
    rclcpp::Publisher<sensor_msgs::msg::Range>::SharedPtr alt_pub;



};



extern int16_t L_Encoder, R_Encoder;
extern float Alt;

#endif /*__SERIAL32_H__*/
